#ifndef _bitstream_std_impl_hpp_
#define _bitstream_std_impl_hpp_

/** \file bitstream_std_impl.hpp
 *  \author Adrian Schweizer
 *  \created  $Sa 18 Aug 03:48:01 pm CEST 2007 schwadri@SchwadriComp.local$
 *  \modified $Do 30 Aug 12:33:54 pm CEST 2007 schwadri@SchwadriComp.local$
 */

#include <algorithm>

namespace core {

    namespace detail {

         template<int V>
            struct exponent_of2
            {
                static const int value  =   exponent_of2<(V>>1)>::value+1;
            };

        template<>
            struct exponent_of2<1>
            {
                static const int value  =   0;
            };

        using std::copy;
        using std::max;
        using std::min;
        using std::swap;

        //BlockType has to be 2^n bytes
        template<class BlockType=unsigned char, class Allocator=std::allocator<BlockType> >
            class bitstream_std_impl
            {
            public:
                typedef BlockType                                   block_type;

                typedef Allocator                                   allocator_type;
                typedef typename allocator_type::size_type          size_type;
                typedef size_type                                   pos_type;
                typedef bitstream_std_impl<BlockType,Allocator>     my_type;

            private:
                static const    size_type                           block_size      =   sizeof(block_type)<<3;          //block size in bits
                static const    size_type                           block_exponent  =   exponent_of2<block_size>::value;//blocksize in bits=2^block_exponent
            public:

                bitstream_std_impl(allocator_type allocator = allocator_type())
                :   m_allocator(allocator),
                    m_begin(0),
                    m_end(0),
                    m_read_offset(0),
                    m_write_offset(0),
                    m_owner(false)
                { }

                bitstream_std_impl(size_type size_bits,allocator_type allocator = allocator_type())
                :   m_allocator(allocator),
                    m_begin(0),
                    m_end(0),
                    m_read_offset(0),
                    m_write_offset(0),
                    m_owner(false)
                {
                    size_type   size_blocks  =   (size_bits+block_size-1)>>block_exponent;
                    if(size_blocks)
                    {
                        m_begin         =   m_allocator.allocate(size_blocks);
                        m_end           =   m_begin + size_blocks;
                        m_owner         =   true;
                    }
                }

                bitstream_std_impl(block_type* data, size_type bit_count, bool copy_data=false,allocator_type allocator = allocator_type())
                :   m_allocator(allocator),
                    m_read_offset(0),
                    m_write_offset(0)
                {
                    size_type   size_blocks  =   (bit_count+block_size-1)>>block_exponent;
                    if(copy_data)
                    {
                        m_owner =   true;
                        m_begin =   m_allocator.allocate(size_blocks);
                        copy(data,data+size_blocks,m_begin);
                        m_end   =   m_begin +   size_blocks;
                        m_write_offset = (m_end-m_begin)<<block_exponent;

                    }
                    else
                    {
                        m_owner =   false;
                        m_begin =   data;
                        m_end   =   data    +   size_blocks;
                        m_write_offset = (m_end-m_begin)<<block_exponent;
                    }
                }

                bitstream_std_impl(my_type& other, bool copy_data=false)
                :   m_allocator(other.m_allocator),
                    m_owner(false),
                    m_begin(0)
                {
                    if(copy_data)
                    {
                        //make this exceptions safe
                        my_type temp(other.size());
                        copy(other.m_begin,other.m_begin+other.size_blocks(),temp.m_begin);

                        //now safely swap out guts

                        swap(temp);
                        m_write_offset  =   other.m_write_offset;
                        m_owner =   true;
                    }
                    else
                    {
                        m_begin         =   other.m_begin;
                        m_end           =   other.m_end;
                        m_write_offset  =   other.m_write_offset;
                        m_owner         =   false;
                    }
                }

                template<class BlockContainer>
                    bitstream_std_impl(BlockContainer& data, bool copy_data=false, allocator_type allocator = allocator_type())
                    :   m_allocator(allocator),
                        m_owner(false),
                        m_read_offset(0),
                        m_write_offset(0),
                        m_begin(0)
                    {
                        if(copy_data)
                        {
                            my_type temp((data.size()*sizeof(typename BlockContainer::value_type))<<3);
                            copy(data.begin(),data.end(),temp.m_begin);
                            temp.m_write_offset = (m_end-m_begin)<<block_exponent;
                            swap(temp);
                            m_owner =   true;
                        }
                        else
                        {
                            m_begin =   data.begin();
                            m_end   =   data.end();
                            m_write_offset = (m_end-m_begin)<<block_exponent;
                        }
                    }

                template<class BlockInputIterator>
                    bitstream_std_impl(BlockInputIterator begin, BlockInputIterator end, allocator_type allocator = allocator_type())
                    :   m_allocator(allocator)
                    {
                        //TODO: implement
                    }

                ~bitstream_std_impl()
                {
                    clear();
                }

                void    reserve(size_type  size_bits)
                {
                    size_type   size  =   (size_bits+block_size-1)>>block_exponent;

                    if(size<= max_size_blocks())
                        return;

                    if(empty())
                    {
                        my_type temp(size_bits);
                        temp.m_write_offset =   m_write_offset;
                        swap(temp);
                    }
                    else
                    {
                        //make this exceptions safe
                        my_type temp(size_bits);
                        copy(m_begin,m_begin+((m_write_offset+block_size-1)>>block_exponent),temp.m_begin);

                        //now safely swap out guts
                        temp.m_write_offset =   m_write_offset;
                        swap(temp);

                    }
                }


                void reset()
                {
                    m_read_offset = m_write_offset = 0;
                }

                void clear()
                {
                    m_read_offset = m_write_offset = 0;
                    if(m_owner)
                    {
                        if(m_begin)
                        {
                            m_allocator.deallocate(m_begin,m_end-m_begin);
                            m_begin     =   m_end   =   0;
                        }
                        m_owner =   false;
                    }
                }

                /** \brief swaps internal variables with another instance of the same class
                 *
                 *  this should never throw an exception
                 */
                void    swap(my_type&   other)    throw()
                {
                    std::swap(m_begin,other.m_begin);
                    std::swap(m_end,other.m_end);
                    std::swap(m_read_offset,other.m_read_offset);
                    std::swap(m_write_offset,other.m_write_offset);
                    std::swap(m_owner,other.m_owner);
                    //swap(m_soze,other.m_size);
                }

                template<class T>
                    void write(const T& var)
                    {
                        if(sizeof(T)>1)
                        {
                        #if !defined(CORE_BITSTREAM_NATIVE_END)
                            T   output  =   core::encoding::swap_endian(var);
                            write_bits(reinterpret_cast<const block_type*>(&output),sizeof(T)<<3,true);
                        #else
                            write_bits(reinterpret_cast<const block_type*>(&var),sizeof(T)<<3,true);
                        #endif
                        }
                        else
                            write_bits(reinterpret_cast<const block_type*>(&var),sizeof(T)<<3,true);

                    }

                template<class T>
                    void write_aligned(const T& var)
                    {
                        if(sizeof(T)>1)
                        {
                        #if !defined(CORE_BITSTREAM_NATIVE_END)
                            T   output  =   core::encoding::swap_endian(var);
                            write_aligned_blocks(reinterpret_cast<const block_type*>(&output),sizeof(T)>>(block_exponent-3));
                        #else
                            write_aligned_blocks(reinterpret_cast<const block_type*>(&var),sizeof(T)>>(block_exponent-3));
                        #endif
                        }
                        else
                            write_aligned_blocks(reinterpret_cast<const block_type*>(&var),sizeof(T)>>(block_exponent-3));

                    }

                void write(bool var)
                {
                    if(var)
                        write1();
                    else
                        write0();
                }

                bool read(bool& var)
                {
                    var =   ((m_begin[m_read_offset>>block_exponent] & (0x01<<(block_size-1-(m_read_offset&(block_size-1)))))>0)?true:false;
                    ++m_read_offset;
                    return true;
                }

                void write0()
                {
                    if((m_write_offset&(block_size-1))==0)
                    {
                        //entering new block. check whether we can write ahead
                        if(m_write_offset >= max_size())
                            reserve(max((max_size()+1)<<1,m_write_offset+block_size));
                        m_begin[m_write_offset>>block_exponent] =   0x0;
                    }
                    ++m_write_offset;
                }

                void write1()
                {
                    if((m_write_offset&(block_size-1))==0)
                    {
                        if(m_write_offset >= max_size())
                            reserve(max((max_size()+1)<<1,m_write_offset+block_size));

                        m_begin[m_write_offset>>block_exponent]     =   0x1 << (block_size-1);
                    }
                    else
                        m_begin[m_write_offset>>block_exponent]     |=  0x1 << ( block_size-1-(m_write_offset&(block_size-1)));
                    ++m_write_offset;
                }

                template<class BlockInputIterator>
                    void write_aligned(BlockInputIterator begin, BlockInputIterator end)
                    {
                        //align to next block_boundary
                        size_type   write_pos_blocks   =   (m_write_offset+block_size-1)>>block_exponent;

                        if((write_pos_blocks+(end-begin))>max_size_blocks())
                            reserve(std::max((max_size_blocks()+1)<<1,(write_pos_blocks+(end-begin))));
                        copy(begin,end,m_begin+ write_pos_blocks);

                        m_write_offset  =  (write_pos_blocks+(end-begin))<<block_exponent;
                    }

                bool read(block_type* output, size_type block_count)
                {
                    return read_bits(output, block_count<<block_exponent,true);
                }

                void write(const block_type* input, size_type block_count)
                {
                    write_bits(input, block_count<<block_exponent,true);
                }

                void write_bits(const block_type *input, size_type bit_count, bool right_aligned)
                {
                    if((m_write_offset+bit_count) > max_size())
                        reserve(max((max_size()+1)<<1,m_write_offset+bit_count));


                    size_type   block_offset   = m_write_offset & (block_size-1);   //the same as: m_write_offset % block_size

                    //block iterator
                    block_type*     oblock_iter=  m_begin + (m_write_offset>>block_exponent);

                    //if offset is 0 this is a simple copy exercice with additional special handling of the last block
                    if(block_offset == 0)
                    {   //in this case its the same as write_aligned

                        size_type   block_count     =   (bit_count+block_size-1)>>block_exponent;
                        copy(input,input+block_count,oblock_iter);
                        m_write_offset              +=  bit_count;
                    }
                    else
                    {
                        const block_type* iblock_iter  =   input;
                        block_type  current_block;
                        size_type   count   =   bit_count;
                        // Faster to put the while at the top surprisingly enough
                        while ( count > (block_size-1) )
                        {
                            current_block = *iblock_iter;
                            // Copy over the new block.
                            *oblock_iter |= current_block >> block_offset; // First half

                            *++oblock_iter = static_cast<block_type>(current_block << (block_size - block_offset)); // Second half (overlaps byte boundary)

                            //m_write_offset  +=  sizeof(block_type)<<3;
                            count       -=  block_size;
                            ++iblock_iter;
                        }

                        if(count >0)
                        {   //a fraction of a block is left
                            current_block   = *iblock_iter;
                            if(right_aligned)
                                current_block   <<= block_size - count; // shift left to get the bits on the left, as in our internal representation
                            *oblock_iter    |= current_block >> block_offset; // First half
                            if(count+block_offset>block_size)
                                *++oblock_iter    =   static_cast<block_type>(current_block << (block_size-block_offset));
                        }
                        m_write_offset  +=  bit_count;

                    }

                }

                template<class ForwardIterator>
                    bool read_aligned(ForwardIterator begin, ForwardIterator end)
                    {
                        //align to next block_boundary
                        size_type   offset   =   (m_read_offset+block_size-1)>>block_exponent;

                        if((offset+(end-begin))>size_blocks())
                            return false;   //not enough data

                        size_type   count   =   end-begin;
                        copy(m_begin+offset,m_begin+offset+count,begin);

                        m_read_offset   =  (offset+count)<<block_exponent;
                        return true;
                    }
                template<class T>
                    bool read(T& var)
                    {
                        #if !defined(CORE_BITSTREAM_NATIVE_END)
                        if(sizeof(T)>1)
                        {
                            bool    retval  = read_bits(reinterpret_cast<block_type*>(&var),sizeof(T)<<3,true);
                            if(retval)
                                var =   core::encoding::swap_endian(var);
                            return retval;
                        }
                        else
                        #endif
                            return read_bits(reinterpret_cast<block_type*>(&var),sizeof(T)<<3,true);

                    }

                template<class T>
                    bool read_aligned(T& var)
                    {
                        #if !defined(CORE_BITSTREAM_NATIVE_END)
                        if(sizeof(T)>1)
                        {
                            bool    retval  = read_aligned_blocks(reinterpret_cast<block_type*>(&var),sizeof(T)>>(block_exponent-3));
                            if(retval)
                                var =   core::encoding::swap_endian(var);
                            return retval;
                        }
                        else
                        #endif
                            return read_aligned_blocks(reinterpret_cast<block_type*>(&var),sizeof(T)>>(block_exponent-3));
                    }

                template<class T>
                    boost::optional<T>   peek() const
                    {
                        T temp;
                        if(do_read_bits(reinterpret_cast<unsigned char*>(&temp),sizeof(T)<<3,true))
                            return temp;

                        return boost::optional<T>();
                    }

                template<class T>
                    boost::optional<T>   peek_aligned() const
                    {
                        T temp;

                        size_type     aligned_read_offset =   (m_read_offset  + (block_size-1)) & ~static_cast<size_type>(block_size-1);

                        if(sizeof(T) > ((size()-aligned_read_offset)>>block_exponent))
                            return boost::optional<T>();

                        const block_type* read_ofs    =   m_begin+(aligned_read_offset>>block_exponent);

                        copy(read_ofs,read_ofs+sizeof(T),reinterpret_cast<unsigned char*>(&temp));
                        return  temp;
                    }

                bool read_bits(block_type* output, size_type bit_count, bool right_aligned)
                {
                    if(do_read_bits(output,bit_count,right_aligned))
                    {
                        m_read_offset              +=  bit_count;
                        return true;
                    }
                    return false;
                }

                bool do_read_bits(block_type* output, size_type bit_count, bool right_aligned) const
                {
                    if (bit_count<=0)
                        return false;

                    if ( m_read_offset + bit_count > size())
                        return false;

                    size_type   block_offset   = m_read_offset & (block_size-1);   //the same as: m_read_offset % block_size



                    //block iterator
                    block_type*     iblock_iter=  m_begin + (m_read_offset>>block_exponent);

                    if(block_offset == 0)
                    {
                        //in this case its the same as read_aligned except that it does not enforce read_offset alignement at the end
                        size_type   block_count     =   (bit_count+block_size-1)>>block_exponent;
                        copy(iblock_iter,iblock_iter+block_count,output);
                    }
                    else
                    {
                        block_type* oblock_iter =   output;
                        size_type   count   =   bit_count;
                        // do
                        // Faster to put the while at the top surprisingly enough
                        while (count > (block_size-1))
                        {
                            *oblock_iter  = *iblock_iter << block_offset; // First half

                            *oblock_iter |= *++iblock_iter >> (block_size-block_offset); // Second half (overlaps byte boundary)

                            count    -= block_size;

                            ++oblock_iter;

                        }
                         if(count >0)
                        {   //a fraction of a block is left
                            *oblock_iter    = static_cast<block_type>(*iblock_iter << block_offset);
                            if((count+block_offset)>block_size)
                                *oblock_iter    |=  *++iblock_iter >> (block_size-block_offset);
                            if(right_aligned)
                                *oblock_iter >>= block_size-count;
                        }
                    }
                    return true;
                }


                void write_aligned_blocks(const block_type* input, size_type block_count)
                {
                    //align to next block_boundary
                    size_type   aligned_write_offset   =   ((m_write_offset  + (block_size-1)) & ~static_cast<size_type>(block_size-1))>>block_exponent;



                    if((aligned_write_offset+block_count)>max_size_blocks())
                        reserve(std::max((max_size_blocks()+1)<<(1+block_exponent),(aligned_write_offset+block_count)<<block_exponent));

                    block_type* write_ofs    =   m_begin+aligned_write_offset;

                    copy(input,input+block_count,write_ofs);

                    m_write_offset  =  (aligned_write_offset+block_count)<<block_exponent;
                }

                bool read_aligned_blocks(block_type* output, size_type block_count)
                {
                    size_type     aligned_read_offset =   (m_read_offset  + (block_size-1)) & ~static_cast<size_type>(block_size-1);
                    if(block_count > (size()-aligned_read_offset)>>block_exponent)
                        return false;

                    block_type* read_ofs    =   m_begin+(aligned_read_offset>>block_exponent);
                    copy(read_ofs,read_ofs+block_count,output);
                    m_read_offset   =   aligned_read_offset+(block_count<<block_exponent);
                    return  true;
                }



                /** bytewise access to data*/
                block_type&  operator[](size_type pos)
                {
                    return m_begin[pos];
                }
                /** bytewise access to data*/
                block_type   operator[](size_type pos) const
                {
                    return m_begin[pos];
                }

                void ignore(size_type bit_count)
                {
                    m_read_offset   =   min(m_read_offset+bit_count,size());
                }

                void ignore()
                {
                    m_read_offset   =  min(m_read_offset+1,size());
                }

                void alignp()
                {
                    //if(m_write_offset & 0x07)   //m_write_offset % 8 != 0
                        //m_write_offset  =   (m_write_offset& ~static_cast<size_type>(0x07))+8;
                    //special case for 8 bits
                    //m_write_offset  =   (m_write_offset  + 0x07) & ~static_cast<size_type>(0x07);
                    //general case
                    m_write_offset  =   (m_write_offset  + (block_size-1)) & ~static_cast<size_type>(block_size-1);
                }

                void aligng()
                {
                    //special case for 8 bits
                    //m_read_offset   =   (m_read_offset  + 0x07) & ~static_cast<size_type>(0x07);
                    //general case
                    m_read_offset   =   (m_read_offset  + (block_size-1)) & ~static_cast<size_type>(block_size-1);
                }

                bool write_offset_block_aligned() const
                {
                    return !(m_write_offset & static_cast<size_type>(block_size-1));
                }

                void seekg(pos_type pos,int rel)
                {
                    switch(rel)
                    {
                    case    0:
                        m_read_offset   +=  pos;
                        break;
                    case    1:
                        m_read_offset   =   pos;
                        break;
                    case    2:
                        m_read_offset   =   m_write_offset-pos;
                        break;
                    }
                }

                pos_type    tellg() const
                {
                    return m_read_offset;
                }

                pos_type    tellp() const
                {
                    return m_write_offset;
                }

                void seekp(pos_type pos,int rel)
                {
                    switch(rel)
                    {
                    case    0:
                        m_write_offset   +=  pos;
                        break;
                    case    1:
                        m_write_offset   =   pos;
                        break;
                    case    2:
                        m_write_offset   -=  pos;
                        break;
                    }
                }

                int copy_data( block_type*& data ) const
                {
                    data    =   new block_type[size_blocks()];
                    copy(m_begin,m_begin+size_blocks(),data);
                    return  m_write_offset;
                }

                block_type* get_data() const
                {
                    return m_begin;
                }

                void set_number_of_bits_allocated(size_type bit_count)
                {
                    m_write_offset  =   bit_count;
                }

                bool        empty() const
                {
                    return  m_write_offset ==  0;
                }

                size_type   size() const
                {
                    return m_write_offset;
                }

                size_type   max_size() const
                {
                    return max_size_blocks()<<block_exponent;
                }

                size_type   max_size_blocks() const
                {
                    return static_cast<size_type>(m_end-m_begin);
                }

                size_type   size_bytes() const
                {
                    return size_blocks()<<(block_exponent-3);
                }

                size_type   size_blocks() const
                {
                    return  static_cast<size_type>(m_write_offset)>>block_exponent;
                }

                operator bool() const
                {
                    return m_read_offset < m_write_offset;
                }

            private:
                allocator_type      m_allocator;
                pos_type            m_read_offset;
                pos_type            m_write_offset;
                block_type*         m_begin;
                block_type*         m_end;

                /** bitstreams that use less than CORE_BITSTREAM_STACK_ALLOCATION_SIZE use the stack, rather than the heap to store data.
                 *  It switches over if CORE_BITSTREAM_STACK_ALLOCATION_SIZE is exceeded
                 */
                //unsigned char   m_stack_data[CORE_BITSTREAM_STACK_ALLOCATION_SIZE];

                bool                m_owner;        //true if we own the data block
            };

    } // namespace detail

} // namespace core
#endif // _bitstream_std_impl_hpp_
